
#include "App/ATK_MS901M/atk_ms901m.h"
#include "App/algorithm.h"
#include "App/basic.h"
#include "App/line_patrol.h"
#include "App/motion.h"
#include "App/motor.h"
#include "App/oled_hardware_i2c.h"
#include "App/servo.h"
#include "App/simple_json.h"
#include <math.h>
#include <ti/driverlib/m0p/dl_interrupt.h>
#define TAG "Main"

atk_ms901m_attitude_data_t attitude_dat;

#define STRAIGHT_LINE_ANGLE 00.0
#define speed 300
void atk_controls_straight_line(float delta_angle) {
  float target_attitude = STRAIGHT_LINE_ANGLE;
  // my_printf("%f\n", target_attitude)
  extern pid_control_t hh;
  int16_t current_ls, current_rs;

  atk_ms901m_get_attitude(&attitude_dat, 1);

  float err = attitude_dat.yaw - target_attitude;

  if (my_abs(err) < 1) {
    my_log("infos ", "err %f cur_yaw %f target %f", err, attitude_dat.yaw,
           target_attitude);
  }
  my_printf("1.%f\n", err);
  int8_t flag = -1;
  if (err > 180.0) {
    err = 360.0 - err;
    flag = 1;
  } else if (err < -180.0) {
    err = -360.0 - err;
    flag = 1;
  } 
    float adjust = pid_update(&hh, (err), 1); // err*2;
  current_ls = speed - flag * adjust;
  current_rs = speed + flag * adjust;
  motion_basic_ctrl(current_ls, current_rs);
  my_printf("2.%f\n", err);
}
int main(void) {
  SYSCFG_DL_init();
  SysTick_Init();
  uart_init();
  OLED_Init();
  motion_init();
  atk_ms901m_init();

  OLED_ShowString(0, 0, "Attitude Test", 16);

  my_log(TAG, "Attitude Test Init Done!");
  // motion_basic_ctrl(300, 300);
  // while( 1 )  {

  // }
  while (1) {
    atk_controls_straight_line(0);
    // atk_ms901m_get_attitude(&attitude_dat, 1);

    // my_printf("Roll: %.02f Pitch: %.02f Yaw: %.02f\r\n", attitude_dat.roll,
    //           attitude_dat.pitch, attitude_dat.yaw);
    mspm0_delay_ms(10);
  }
}

// void _atk_controls_straight_line(float delta_angle) {
//   float target_attitude = STRAIGHT_LINE_ANGLE;
//   my_printf("%f\n", target_attitude) extern pid_control_t hh;
//   int16_t current_ls, current_rs;

//   atk_ms901m_get_attitude(&attitude_dat, 1);

//   if (fabs(target_attitude - attitude_dat.yaw) < 1) {
//     current_ls = speed;
//     current_rs = speed;
//   } else {
//     while (fabs(target_attitude - attitude_dat.yaw) < 1) {
//       atk_ms901m_get_attitude(&attitude_dat, 1);
//       float adjust = pid_update(&hh, target_attitude - attitude_dat.yaw, 1);
//       current_ls = speed + adjust;
//       current_rs = speed - adjust;
//       motion_basic_ctrl(current_ls, current_rs);
//     }
//   }

//   motion_basic_ctrl(current_ls, current_rs);
// }
